#include "stepmotor.h"
#include "OLED.h"
#define abs(x) ((x)>0?(x):-(x))
//L298N
uint16_t pulse_num=0;
uint16_t pulse1=0;
uint16_t pulse2=0;


//void motor_front(int num,int16_t angle)//步进角7.2
//{
//	for(pulse_num=0;pulse_num<(float)angle/1.8/4.0;pulse_num++)
//	{
//		Mot1F;Mot2B;Mot3B;Mot4F;
//		HAL_Delay(num);
//		Mot1F;Mot2B;Mot3F;Mot4B;
//		HAL_Delay(num);
//		Mot1B;Mot2F;Mot3F;Mot4B;
//		HAL_Delay(num);
//		Mot1B;Mot2F;Mot3B;Mot4F;
//		HAL_Delay(num);
//	}
//}

//void motor_back(int num)//
//{
//	Mot1B;Mot2B;Mot3B;Mot4F;
//	HAL_Delay(num);
//	Mot1B;Mot2F;Mot3B;Mot4B;
//	HAL_Delay(num);
//	Mot1B;Mot2B;Mot3F;Mot4B;
//	HAL_Delay(num);
//	Mot1F;Mot2B;Mot3B;Mot4B;
//	HAL_Delay(num);
//}

//void motor_half_front(int num)//3.6
//{
//	Mot1F;Mot2B;Mot3B;Mot4B;
//	HAL_Delay(num);
//	Mot1F;Mot2B;Mot3F;Mot4B;
//	HAL_Delay(num);
//	Mot1B;Mot2B;Mot3F;Mot4B;
//	HAL_Delay(num);
//	Mot1B;Mot2F;Mot3F;Mot4B;
//	HAL_Delay(num);
//	Mot1B;Mot2F;Mot3B;Mot4B;
//	HAL_Delay(num);
//	Mot1B;Mot2F;Mot3B;Mot4F;
//	HAL_Delay(num);
//	Mot1B;Mot2B;Mot3B;Mot4F;
//	HAL_Delay(num);
//	Mot1F;Mot2B;Mot3B;Mot4F;
//	HAL_Delay(num);
//}
////����ʹ��L298N����������ʱ�����Mot1B;Mot2B;Mot3B;Mot4B;��

void moto_init(MotorType_t *motor, uint8_t id) {
	motor->id = id;
	motor->angle = 0;
	motor->realAngle = 0;
	motor->targetAngle = 0;
}

/**
 * @brief 步进电机速度设置函数
 * 
 * @param speed 速度，单位脉冲每秒
 * @param motor 
 */
void moto_set_speed(float speed, MotorType_t motor)
{
	if(abs(speed)<=0.00001) {
		if (motor.id == MOTOR_HORIZONTAL_MOVE) {
			MotStop2;
		} else if (motor.id == MOTOR_VERTICAL_MOVE) {
			MotStop1;
		}
		return;
	}
	uint8_t dir = speed > 0 ? 0 : 1; // 方向
	speed = abs(speed);
	float arr = 320000/ (float)speed-1; // 计算arr
	uint16_t load;
	if(arr<UINT16_MAX){
		load = (uint16_t)arr;
		if(load<4) load = 4; // 最小值为3
		if (motor.id == MOTOR_HORIZONTAL_MOVE) {
			__HAL_TIM_SetAutoreload(TM_M2, load);
			if((int)(TM_M2)->Instance->CNT>(int)(TM_M2)->Init.Period-10){
				__HAL_TIM_SetCounter(TM_M2, 0); // 重置计数器
			}
			if (dir == 0) {
				MotFront2;
			} else {
				MotBack2;
			}
			MotStart2;
		} else if (motor.id == MOTOR_VERTICAL_MOVE) {
			__HAL_TIM_SetAutoreload(TM_M1, load);
			if((int)(TM_M1)->Instance->CNT>(int)(TM_M1)->Init.Period-10){
				__HAL_TIM_SetCounter(TM_M1, 0); // 重置计数器
			}
			if (dir == 0) {
				MotFront1;
			} else {
				MotBack1;
			}
			MotStart1;
		}
	}
	else {
		if (motor.id == MOTOR_HORIZONTAL_MOVE) {
			MotStop2;
		} else if (motor.id == MOTOR_VERTICAL_MOVE) {
			MotStop1;
		}
		return;
	}
}

// void moto_set_angle1(int32_t angle)
// {
// //	MotStop;
// //	HAL_Delay(20);
// 	if(angle>0){
		
// 		pulse1=(float)angle/(36000.0/(float)subdivision);
// 		MotAngleStart1;
// 		MotFront1;
// //		printf("pulse=%d\n",pulse1);
// 	}
// 	else if(angle<0){
// 		pulse1=(float)(-angle)/(36000.0/(float)subdivision);
// 		MotAngleStart1;
// 		MotBack1;
		
// 		//OLED_ShowNum(1,1,pulse,4);
// 	}
// 	else
// 		MotAngleStop1;
// }

// void moto_set_angle2(int32_t angle)
// {
// //	MotStop;
// //	HAL_Delay(20);
// 	if(angle>0){
// 		pulse2=(float)angle/(36000.0/(float)subdivision);
// 		MotAngleStart2;
// 		MotFront2;
		
// 		//OLED_ShowNum(1,1,pulse,4);
// 	}
// 	else if(angle<0){
// 		pulse2=(float)(-angle)/(36000.0/(float)subdivision);
// 		MotAngleStart2;
// 		MotBack2;
		
// 		//OLED_ShowNum(1,1,pulse,4);
// 	}
// 	else
// 		MotAngleStop2;
// }

void moto_set_angle(int32_t angle, MotorType_t motor)
{
	if (motor.id == MOTOR_HORIZONTAL_MOVE) {
		if(angle>0){
		
			pulse1=(float)angle/(36000.0/(float)subdivision);
			MotAngleStart2;
			MotFront2;
	//		printf("pulse=%d\n",pulse1);
		}
		else if(angle<0){
			pulse1=(float)(-angle)/(36000.0/(float)subdivision);
			MotAngleStart2;
			MotBack2;
			
			//OLED_ShowNum(1,1,pulse,4);
		}
		else
			MotAngleStop2;
			
	} else if (motor.id == MOTOR_VERTICAL_MOVE) {
		if(angle>0){
			pulse2=(float)angle/(36000.0/(float)subdivision);
			MotAngleStart1;
			MotFront1;
			
			//OLED_ShowNum(1,1,pulse,4);
		}
		else if(angle<0){
			pulse2=(float)(-angle)/(36000.0/(float)subdivision);
			MotAngleStart1;
			MotBack1;
			
			//OLED_ShowNum(1,1,pulse,4);
		}
		else
			MotAngleStop1;
	}
}



void HAL_TIM_PWM_PulseFinishedCallback(TIM_HandleTypeDef *htim) 
{
	if(htim->Instance==TIM2&&(pulse1)!=0)
	{
		pulse1--;
		if(pulse1==1)
		{

			MotStop1;
			pulse1=0;
		}
	}
	if(htim->Instance==TIM3&&(pulse2)!=0)
	{
		pulse2--;
		if(pulse2==1)
		{
			MotStop2;
			pulse2=0;
		}
	}
}

